{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 贝塞尔曲线实现车辆轨迹规划"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import copy\n",
    "from celluloid import Camera  # 保存动图时用，pip install celluloid\n",
    "%matplotlib qt5\n",
    "\n",
    "from n_Bezier import bezier # 导出贝塞尔曲线生成函数"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 参数设置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "d = 3.5  # 道路标准宽度\n",
    "\n",
    "\n",
    "# 控制点\n",
    "Ps = np.array([\n",
    "    [0, -d / 2],\n",
    "    [25, -d / 2],\n",
    "    [25, d / 2],\n",
    "    [50, d / 2]\n",
    "    ])\n",
    "\n",
    "n = len(Ps) - 1  # 贝塞尔曲线的阶数\n",
    "\n",
    "path=[]  # 路径点存储\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 贝塞尔点求解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "for t in np.arange(0,1.01,0.01):\n",
    "    p_t = bezier(Ps,len(Ps),t)\n",
    "    path.append(p_t)\n",
    "path = np.array(path)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 画图"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "MovieWriter ffmpeg unavailable; using Pillow instead.\n"
     ]
    }
   ],
   "source": [
    "\n",
    "\n",
    "\n",
    "## 画图\n",
    "fig = plt.figure(1)\n",
    "# plt.ylim(-4, 4)\n",
    "# plt.axis([-10, 100, -15, 15])\n",
    "camera = Camera(fig)\n",
    "len_line = 50\n",
    "# 画灰色路面图\n",
    "GreyZone = np.array([[- 5, - d - 0.5], [- 5, d + 0.5],\n",
    "                     [len_line, d + 0.5], [len_line, - d - 0.5]])\n",
    "for i in range(len(path)):\n",
    "    # plt.cla()\n",
    "\n",
    "    plt.fill(GreyZone[:, 0], GreyZone[:, 1], 'gray')\n",
    "    # 画分界线\n",
    "    plt.plot(np.array([- 5, len_line]), np.array([0, 0]), 'w--')\n",
    "\n",
    "    plt.plot(np.array([- 5, len_line]), np.array([d, d]), 'w')\n",
    "\n",
    "    plt.plot(np.array([- 5, len_line]), np.array([- d, - d]), 'w')\n",
    "\n",
    "    plt.plot(Ps[:,0],Ps[:,1],'ro')\n",
    "    plt.plot(Ps[:, 0], Ps[:, 1], 'y')\n",
    "    # 设置坐标轴显示范围\n",
    "    # plt.axis('equal')\n",
    "    plt.gca().set_aspect('equal')\n",
    "    # 绘制路径\n",
    "\n",
    "    plt.plot(path[0:i, 0], path[0:i, 1], 'g')  # 路径点\n",
    "    plt.pause(0.001)\n",
    "    camera.snap()\n",
    "animation = camera.animate()\n",
    "animation.save('trajectory.gif')\n"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
